# ----------------------------
# coding: utf-8
# 开发作者 : 钱嘉龙
# 创建日期 : 2022/2/22 15:18
# ----------------------------

import cv2 as cv
import numpy as np
from cameralib import ColorDetection
import math
import serial
import threading
from copy import deepcopy

global thread_exit
thread_lock = threading.Lock()
thread_exit = False


class myThread(threading.Thread):
    def __init__(self, camera_id, img_height, img_width):
        super(myThread, self).__init__()
        self.camera_id = camera_id
        self.img_height = img_height
        self.img_width = img_width
        self.frame = np.zeros((img_height, img_width, 3), dtype=np.uint8)

    def get_frame(self):
        return deepcopy(self.frame)

    def run(self):
        global thread_exit
        cap = cv.VideoCapture(self.camera_id)
        while not thread_exit:
            ret, frame = cap.read()
            if ret:
                frame = cv.resize(frame, (self.img_width, self.img_height))
                thread_lock.acquire()
                self.frame = frame
                thread_lock.release()
            else:
                thread_exit = True
        cap.release()

# # 多线程的主函数
# def main():
#     global thread_exit
#     # 定义初值
#     camera_id1 = 0
#     camera_id2 = 2
#     img_height = 480
#     img_width = 640
#     # 配置类
#     thread1 = myThread(camera_id1, img_height, img_width)
#     thread2 = myThread(camera_id2, img_height, img_width)
#     thread1.start()
#     thread2.start()
#
#     while not thread_exit:
#         # 上锁
#         thread_lock.acquire()
#         frame1 = thread1.get_frame()
#         frame2 = thread2.get_frame()
#         # 解锁
#         thread_lock.release()
#         cv.imshow('frame1', frame1)
#         cv.imshow('frame2', frame2)
#         if frame1 is None:
#             print("camera1 open failed!")
#         if frame2 is None:
#             print("camera2 open failed!")
#         # if not Open_flag:
#             # usart1()
#         # camera(frame)
#         if cv.waitKey(1) == 27:
#             thread_exit = True
#     # 等待线程结束
#     thread1.join()



# 串口发送小车移动方向
def camera_move_control(right_or_left, front_or_behind):
    move = ''
    if right_or_left == 0 and front_or_behind == 0:
        move += '0'
        move += '0'
        msg = move
        ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.01)
        while True:
            for i in range(0, 2):
                ser.write(bytes('D', encoding='utf-8'))
                ser.write(bytes(msg, encoding='utf-8'))
            print("position is right and move control send success!")
            break
        return 1
    else:
        move += str(right_or_left)
        move += str(front_or_behind)
        msg = move
        ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.01)
        while True:
            for i in range(0, 2):
                ser.write(bytes('D', encoding='utf-8'))
                ser.write(bytes(msg, encoding='utf-8'))
            print("move control send success!")
            break
        return 1


def camera_process(image, image2):
    process_num = 1
    debug_flag = 1
    base_point = [330, 90]
    detect_QRcode_flag = 0
    send_QRcode_flag = 0
    dtlocation_1 = 0
    dtlocation_2 = 0
    dtlocation_3 = 0
    dtlocation_4 = 0
    sdlocation_1 = 0
    sdlocation_2 = 0
    sdlocation_3 = 0
    sdlocation_4 = 0
    sdlocation_num = 0
    dtcolor_1 = 0
    sdcolor_1 = 0
    dtcolor_2 = 0
    sdcolor_2 = 0
    dtcolor_2_delay_num = 0
    dtcolor_2_delay_flag = 0
    t = 0
    barcodeData = ''
    QRcode_box = []
    color_position = []
    mission = ''
    grab_pos = ''
    right_or_left = 0
    front_or_behind = 0
    center = []
    distance = 0
    red_lower = np.array([[0, 60, 60], [130, 60, 60]])
    red_upper = np.array([[4, 255, 255], [180, 255, 255]])
    # 红绿蓝
    lower = np.array([red_lower[0], [40, 80, 80], [100, 150, 55]])
    upper = np.array([red_upper[0], [80, 255, 255], [125, 255, 255]])

    # cap1 = cv.VideoCapture(2)
    # cap2 = cv.VideoCapture(1)
    # while True:
    # 等待识别二维码
    while detect_QRcode_flag == 0 and send_QRcode_flag == 0 and debug_flag == 0:
        grab_pos = ''
        print('waiting start......')
        detect_QRcode_flag = cam.usart_wait_open()
        if detect_QRcode_flag:
            break
    # 等待识别一区物块颜色
    while send_QRcode_flag and dtcolor_1 == 0 and sdcolor_1 == 0 and debug_flag == 0:
        grab_pos = ''
        color_position = ['', '', '']
        print('waiting detect color No.1 ......')
        dtcolor_1 = cam.usart_wait_detect_color_1()
        if dtcolor_1:
            break
    # 等待一区物块定位1
    while sdcolor_1 and dtlocation_1 == 0 and sdlocation_1 == 0 and debug_flag == 0:
        print('waiting dtlocation_1 ......')
        dtlocation_1 = cam.usart_wait_detect_location_1()
        if dtlocation_1:
            break
    # 等待一区物块定位2
    while sdlocation_1 and dtlocation_2 == 0 and sdlocation_2 == 0 and debug_flag == 0:
        print('waiting dtlocation_2 ......')
        dtlocation_2 = cam.usart_wait_detect_location_1()
        if dtlocation_2:
            break
    # 等待识别二区物块颜色
    while sdlocation_2 and dtcolor_2 == 0 and sdcolor_2 == 0 and debug_flag == 0:
        grab_pos = ''
        color_position = ['', '', '']
        print('waiting detect color No.2 ......')
        dtcolor_2 = cam.usart_wait_detect_color_2()
        if dtcolor_2:
            break
    # 等待二区物块定位1
    while sdcolor_2 and dtlocation_3 == 0 and sdlocation_3 == 0 and debug_flag == 0:
        print('waiting dtlocation_3 ......')
        dtlocation_3 = cam.usart_wait_detect_location_1()
        if dtlocation_3:
            break
    # 等待二区物块定位2
    while sdlocation_3 and dtlocation_4 == 0 and sdlocation_4 == 0 and debug_flag == 0:
        print('waiting dtlocation_4 ......')
        dtlocation_4 = cam.usart_wait_detect_location_1()
        if dtlocation_4:
            break

    if debug_flag == 0:
        if (dtlocation_1 == 1 and sdlocation_1 == 0) or (dtlocation_2 == 1 and sdlocation_2 == 0)\
                or (dtlocation_3 == 1 and sdlocation_3 == 0) or (dtlocation_4 == 1 and sdlocation_4 == 0):
            process_num = 2
        else:
            process_num = 1

    print('process_num', process_num)
    if process_num == 1:
        # t = cv.getTickCount()
        # 创建卷积核
        kernel_rect_3 = cam.get_kernel(0, 3)
        # kernel_rect_5 = cam.get_kernel(0, 5)

        # # 获取摄像头图像并获取相关区域
        # ret, image = cam.camera_get_images(cap1)
        # if not ret:
        #     print('No Camera 1')
        #     continue
        #
        # ret2, image2 = cap2.read()
        # if not ret2:
        #     print('No Camera2')
        #     continue

        cv.imshow('1', image)
        cv.imshow('2', image2)

        # image = cam.get_images('sum')
        # cam.show_image('in', image, 0, True)
        image = cv.resize(image, (640, 480))
        # image = image[0:648][180:330]

        # 调整亮度以及滤波
        # image_brightness = cam.contrast_brightness(image, 1, 0)
        # blurred = cam.gaussian_blur(image_brightness, 5)
        gray = cam.convert_gray_scale(image)

        ####################################################################
        if detect_QRcode_flag == 1 and send_QRcode_flag == 0:
            QRcode_data = cam.detect_QR_code(gray)
            if QRcode_data == 0:
                print('No QRcode!')
            else:
                barcodeData = QRcode_data[0]
                QRcode_box = QRcode_data[1]
                if barcodeData is not (None or ''):
                    mission = barcodeData

        # print(barcodeData, QRcode_box)
        # cam.show_image('QRcode_img', QRcode_img, 0, False)
        # binary = cam.threshold(gary)

        # RGB转HSV
        hsv_image = cv.cvtColor(image, cv.COLOR_BGR2HSV)
        color_position = ['', '', '']
        dst_image = np.zeros_like(hsv_image)
        mask_red = cv.inRange(hsv_image, red_lower[1], red_upper[1])
        print('---------------------------------------------')
        for i in range(0, 3):
            mask = cv.inRange(hsv_image, lower[i], upper[i])
            if i == 0:
                mask = cv.bitwise_or(mask_red, mask)
            # 对获取的mask进行形态学操作
            # morphology_image = cam.morphology_process(mask, 0, kernel_rect_3, 3)
            # morphology_image = cam.morphology_process(morphology_image, 1, kernel_rect_3, 3)

            # 寻找外轮廓
            contours = cv.findContours(mask, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)[0]
            # blurred_copy1 = image.copy()
            # cv.drawContours(blurred_copy1, contours, -1, (255, 255, 255), 1)

            # 获取最小外接矩形以及其他数据
            blurred_copy2 = image.copy()
            # 显示返回图
            min_bounding_boxes = cam.min_bounding_boxes(contours, blurred_copy2, number=i)[0]
            # 物块对应的颜色
            color = cam.min_bounding_boxes(contours, blurred_copy2, number=i)[2]
            # 每个物块的中心点坐标
            (core_x, core_y) = cam.min_bounding_boxes(contours, blurred_copy2, number=i)[5]
            dst_image = cv.bitwise_or(min_bounding_boxes, dst_image)
            color_position = ['', '', '']
            color_position = cam.decide_position(color, core_x)


        # if QRcode_data != 0:
        #     dst_image = cam.QRcode_draw(dst_image, QRcode_box, barcodeData)
        # cam.show_image('dst_image', dst_image, 0, False)
        if dtcolor_2:
            dtcolor_2_delay_num += 1
            if dtcolor_2_delay_num == 20:
                dtcolor_2_delay_flag = 1
                dtcolor_2_delay_num = 0
        # print(color_position, type(color_position))
        # print(color_position == (None or ['', '', '']))
        if (mission is not (None or '')) and (color_position != (None or ['', '', ''])):
            grab_pos = cam.mission_process(mission, color_position)
        print('color_position', color_position)
        # print('mission', mission)
        print("grab_pos:", grab_pos)

        if mission is not (None or '') and detect_QRcode_flag and send_QRcode_flag == 0:
            send_QRcode_flag = cam.usart_send_QRcode(mission)
        if grab_pos != '' and dtcolor_1 and sdcolor_1 == 0:
            sdcolor_1 = cam.usart_send_color_1(grab_pos)
        if grab_pos != '' and dtcolor_2_delay_flag and sdcolor_2 == 0:
            sdcolor_2 = cam.usart_send_color_2(grab_pos)
        # while sdcolor_2:
        #     print('mission success!')
        #     while True:
        #         exit_flag = 1


        # Esc退出
        # key = cv.waitKey(1)
        # if key == 27:
        #     break
        # t = (cv.getTickCount()-t)/cv.getTickFrequency()
        # fps = 1/t
        # print('FPS:', int(fps))
    elif process_num == 2:
        right_or_left = -1
        front_or_behind = -1
        center = []
        distance = 0

        # 获取摄像头图像并获取相关区域
        # ret, image2 = cap2.read()
        # if not ret:
        #     print('No Camera2')
        #     continue

        # image = cv.imread('F:\\EngineeringDocuments\\Logistics_Handling_Robot\\images\\blue.jpg')
        # image2 = cv.resize(image2, (640, 480))

        # 调整亮度以及滤波
        # image_brightness = cam.contrast_brightness(image, 1, 0)
        # blurred = cam.gaussian_blur(image, 5)
        # gary = cam.convert_gray_scale(blurred)
        # binary = cam.threshold(gary)

        # RGB转HSV
        hsv_image2 = cv.cvtColor(image2, cv.COLOR_BGR2HSV)
        dst_image2 = np.zeros_like(hsv_image2)

        mask_red2 = cv.inRange(hsv_image2, red_lower[1], red_upper[1])
        print('---------------------------------------------')
        for i in range(0, 3):
            mask2 = cv.inRange(hsv_image2, lower[i], upper[i])
            if i == 0:
                mask2 = cv.bitwise_or(mask_red2, mask2)
            # if i == 1:
            #     cam_2.show_image('mask_', mask, 0, False)
            # 对获取的mask进行形态学操作
            # morphology_image = cam.morphology_process(mask, 0, kernel_rect_3, 1)
            # morphology_image = cam.morphology_process(morphology_image, 1, kernel_rect_3, 1)

            output = np.zeros((mask2.shape[0], mask2.shape[1], 3), np.uint8)
            num_labels, labels, stats, centroids = cv.connectedComponentsWithStats(mask2, connectivity=4)
            point_num = -1
            for index, stat in enumerate(stats):
                # print('area:', stat[4])
                if 8000 < stat[4] < 40000:
                    point_num = index
                    break
            if point_num == -1 or point_num == 0:
                continue
            # print('point_num:', point_num)
            for j in range(1, num_labels):
                mask = labels == j
                output[:, :, 0][mask] = np.random.randint(0, 255)
                output[:, :, 1][mask] = np.random.randint(0, 255)
                output[:, :, 2][mask] = np.random.randint(0, 255)
            cv.imshow('0', output)
            # print(centroids[point_num])
            if centroids[point_num] is not None:
                # distance = cam_2.calculate_long(centroids[point_num], base_point)
                distance = math.pow((centroids[point_num][0] - base_point[0]), 2) + math.pow(
                    (centroids[point_num][1] - base_point[1]), 2)
                distance = math.sqrt(distance)

                if distance > 15:
                    # 小车向左: 1       小车向右: 2
                    right_or_left = 0
                    # 小车向前: 1       小车向后: 2
                    front_or_behind = 0
                    if centroids[point_num][0] < base_point[0]:
                        right_or_left = 1
                    else:
                        right_or_left = 2
                    if centroids[point_num][1] < base_point[1]:
                        front_or_behind = 1
                    else:
                        front_or_behind = 2
                    # right_or_left, front_or_behind = cam_2.pos_adjust(centroids[point_num], base_point)
            # print('center', centroids[point_num])
            print('i', i)

        print('right_or_left:', right_or_left, 'front_or_behind', front_or_behind)
        if right_or_left == 0 and front_or_behind == 0:
            return
        # print('distance', distance)
        # cam.show_image('dst_image', dst_image, 0, False)
        if dtlocation_1 == 1 and sdlocation_1 == 0:
            if right_or_left != 0 and front_or_behind != 0:
                camera_move_control(right_or_left, front_or_behind)
            elif right_or_left == 0 and front_or_behind == 0:
                sdlocation_1 = 1
        elif dtlocation_2 == 1 and sdlocation_2 == 0:
            if right_or_left != 0 and front_or_behind != 0:
                camera_move_control(right_or_left, front_or_behind)
            elif right_or_left == 0 and front_or_behind == 0:
                sdlocation_2 = 1
        elif dtlocation_3 == 1 and sdlocation_3 == 0:
            if right_or_left != 0 and front_or_behind != 0:
                camera_move_control(right_or_left, front_or_behind)
            elif right_or_left == 0 and front_or_behind == 0:
                sdlocation_3 = 1
        elif dtlocation_4 == 1 and sdlocation_4 == 0:
            if right_or_left != 0 and front_or_behind != 0:
                camera_move_control(right_or_left, front_or_behind)
            elif right_or_left == 0 and front_or_behind == 0:
                sdlocation_4 = 1
        # cv.waitKey(0)
        # Esc退出
        # key = cv.waitKey(1)
        # if key == 27:
        #     break


if __name__ == "__main__":
    cam = ColorDetection()

    # 定义初值
    camera_id1 = 1
    camera_id2 = 2
    img_height = 480
    img_width = 640
    # 配置类
    thread1 = myThread(camera_id1, img_height, img_width)
    thread2 = myThread(camera_id2, img_height, img_width)
    thread1.start()
    thread2.start()

    while not thread_exit:
        # 上锁
        thread_lock.acquire()
        frame1 = thread1.get_frame()
        frame2 = thread2.get_frame()
        # 解锁
        thread_lock.release()
        # cv.imshow('frame1', frame1)
        # cv.imshow('frame2', frame2)
        if frame1 is None:
            print("camera1 open failed!")
        if frame2 is None:
            print("camera2 open failed!")
        # if not Open_flag:
            # usart1()
        # camera(frame)
        camera_process(frame1, frame2)
        if cv.waitKey(1) == 27:
            thread_exit = True
    # 等待线程结束
    thread1.join()
    thread2.join()

    # while True:
    #     distance_l = cam.get_laser_distance()
    #     print(distance_l, type(distance_l))
